﻿#include "thesct.h"

PCTSTHPSV g_tsthpsv_list = NULL;

int Sign(int i)
{
    int r = 0;
    if (i > 0) r = 1;
    if (i < 0) r = -1;
    return r;
}

int get_max_neighbour(POINT &p)
{
    int i = 0;
    /*
    if (p.b_up > -1) i++;
    if (p.b_down > -1) i++;
    if (p.b_left > -1) i++;
    if (p.b_right > -1) i++;
    if (p.b_lu > -1) i++;
    if (p.b_ld > -1) i++;
    if (p.b_ru > -1) i++;
    if (p.b_rd > -1) i++;
    */
    i = 8;
    return i;
}
int CreateTSTHPSV(CVisionMatterElementSet &vmes,POINT &point1, POINT &point2,PTSTHPSV *ppTSTHPSV) // 由两个位置上连续的点，构建一个视觉同一物点集
{
    int i_err = 0;
    PTSTHPSV pTSTHPSV = NULL;
    PEPOINT pep1 = NULL;PEPOINT pep2 = NULL;
    PTPOINT ptp1 = NULL;PTPOINT ptp2 = NULL;
    PTPOINT ptp12 = NULL;PTPOINT ptp22 = NULL;

    PSVisionSymbolElement pvmes = vmes.get_vmes_data();

    POINT p1, p2;

    if (point1.n > point2.n) {
        p1 = point2;p2 = point1;
    } else {
        p1 = point1;p2 = point2;
    }


    if ( (p1.x < 0) || (p1.x > vmes.get_width()) ) {
        i_err = -4; // 参数错误，p1不是vmes范围内的点
        goto error_tray;
    }
    if ( (p1.y < 0) || (p1.y > vmes.get_height()) ) {
        i_err = -4; // 参数错误，p1不是vmes范围内的点
        goto error_tray;
    }
    if ( (p2.x < 0) || (p2.x > vmes.get_width()) ) {
        i_err = -4; // 参数错误，p2不是vmes范围内的点
        goto error_tray;
    }
    if ( (p2.y < 0) || (p2.y > vmes.get_height()) ) {
        i_err = -4; // 参数错误，p2不是vmes范围内的点
        goto error_tray;
    }

    if ( ( abs(p1.x - p2.x) > 1 ) || ( abs(p1.y - p2.y) > 1 ) ) {
            i_err = -3; // 参数错误，p1,p2不是两个连续的点
            goto error_tray;
    }
    if ( ((p1.x == p2.x) && (p1.y == p2.y)) || (p1.n == p2.n) ) {
        i_err = -2; // p1与p2是同一个位置上的点
        goto error_tray;
    }

    // 分配内存
    pTSTHPSV = new TSTHPSV();
    if (pTSTHPSV == NULL) {i_err = -1;goto error_tray;} // 内存不足

    pep1 = new EPOINT();
    pep2 = new EPOINT();
    if (pep1 == NULL) {i_err = -1;goto error_tray;} // 内存不足
    if (pep2 == NULL) {i_err = -1;goto error_tray;} // 内存不足

    pep1->next = pep1->prev = NULL;
    pep1->point = p1;
    pep2->next = pep2->prev = NULL;
    pep2->point = p2;

    pep1->point.b_up = pep1->point.b_down = 0;
    pep1->point.b_left = pep1->point.b_right = 0;
    pep1->point.b_lu = pep1->point.b_ld = 0;
    pep1->point.b_ru = pep1->point.b_rd = 0;

    pep1->point.up = pep1->point.down = NULL;
    pep1->point.left = pep1->point.right = NULL;
    pep1->point.ld = pep1->point.lu = NULL;
    pep1->point.ru = pep1->point.rd = NULL;

    pep1->point.iCurNeighbour = 1;

    pep2->point.b_up = pep2->point.b_down = 0;
    pep2->point.b_left = pep2->point.b_right = 0;
    pep2->point.b_lu = pep2->point.b_ld = 0;
    pep2->point.b_ru = pep2->point.b_rd = 0;

    pep2->point.up = pep2->point.down = NULL;
    pep2->point.left = pep2->point.right = NULL;
    pep2->point.ld = pep2->point.lu = NULL;
    pep2->point.ru = pep2->point.rd = NULL;

    pep2->point.iCurNeighbour = 1;

    if (pep1->point.x - 1 < 0) {
        pep1->point.b_ld = pep1->point.b_lu =
                pep1->point.b_left = -1;pep1->point.b_lu = -1;pep1->point.b_ld = -1;
    }
    if (pep1->point.x + 1 >= vmes.get_width()) {
        pep1->point.b_rd = pep1->point.b_ru =
                pep1->point.b_right = -1;pep1->point.b_ru = -1;pep1->point.b_rd = -1;
    }
    if (pep1->point.y - 1 < 0) {
        pep1->point.b_lu = pep1->point.b_ru =
                pep1->point.b_up = -1;pep1->point.b_lu = -1;pep1->point.b_ru = -1;
    }
    if (pep1->point.y + 1 >= vmes.get_height()) {
        pep1->point.b_ld = pep1->point.b_rd =
                pep1->point.b_down = -1;pep1->point.b_ld = -1;pep1->point.b_rd = -1;
    }

    if (pep2->point.x - 1 < 0) {
        pep2->point.b_ld = pep2->point.b_lu =
                pep2->point.b_left = -1;pep2->point.b_lu = -1;pep2->point.b_ld = -1;
    }
    if (pep2->point.x + 1 >= vmes.get_width()) {
        pep2->point.b_rd = pep2->point.b_ru =
                pep2->point.b_right = -1;pep2->point.b_ru = -1;pep2->point.b_rd = -1;
    }
    if (pep2->point.y - 1 < 0) {
        pep2->point.b_lu = pep2->point.b_ru =
                pep2->point.b_up = -1;pep2->point.b_lu = -1;pep2->point.b_ru = -1;
    }
    if (pep2->point.y + 1 >= vmes.get_height()) {
        pep2->point.b_ld = pep2->point.b_rd =
                pep2->point.b_down = -1;pep2->point.b_ld = -1;pep2->point.b_rd = -1;
    }

    if ( (p2.x == p1.x) && (p2.y == p1.y + 1) ) {        // p2在p1下
        pep1->point.b_down = 1;
        pep1->point.down = &(pep2->point);

        pep2->point.b_up = 1;
        pep2->point.up = &(pep1->point);

    } else if ( (p2.x == p1.x) && (p2.y == p1.y - 1) ) { // p2在p1上
        pep1->point.b_up = 1;
        pep1->point.up = &(pep2->point);

        pep2->point.b_down = 1;
        pep2->point.down = &(pep1->point);
    } else if ( (p2.x == p1.x + 1) && (p2.y == p1.y) ) { // p2在p1右
        pep1->point.b_right = 1;
        pep1->point.right = &(pep2->point);

        pep2->point.b_left = 1;
        pep2->point.left = &(pep1->point);
    } else if ( (p2.x == p1.x - 1) && (p2.y == p1.y) ) { // p2在p1左
        pep1->point.b_left = 1;
        pep1->point.left = &(pep2->point);

        pep2->point.b_right = 1;
        pep2->point.right = &(pep1->point);
    } else if ( (p2.x == p1.x + 1) && (p2.y == p1.y + 1) ) { // p2在p1右下
        pep1->point.b_rd = 1;
        pep1->point.rd = &(pep2->point);

        pep2->point.b_lu = 1;
        pep2->point.lu = &(pep1->point);
    } else if ( (p2.x == p1.x - 1) && (p2.y == p1.y - 1) ) { // p2在p1左上
        pep1->point.b_lu = 1;
        pep1->point.lu = &(pep2->point);

        pep2->point.b_rd = 1;
        pep2->point.rd = &(pep1->point);
    } else if ( (p2.x == p1.x + 1) && (p2.y == p1.y - 1) ) { // p2在p1右上
        pep1->point.b_ru = 1;
        pep1->point.ru = &(pep2->point);

        pep2->point.b_ld = 1;
        pep2->point.ld = &(pep1->point);
    } else if ( (p2.x == p1.x - 1) && (p2.y == p1.y + 1) ) { // p2在p1左下
        pep1->point.b_ld = 1;
        pep1->point.ld = &(pep2->point);

        pep2->point.b_ru = 1;
        pep2->point.ru = &(pep1->point);
    }

    pep1->point.iMaxNeighbour = get_max_neighbour(pep1->point);
    pep2->point.iMaxNeighbour = get_max_neighbour(pep2->point);

    pep1->next = pep2;pep2->next = NULL;
    pep1->prev = NULL;pep2->prev = pep1;
    pTSTHPSV->pEPS = pep1;

    pep1->n_crossover = pep2->n_crossover = 0;

    ptp1 = new TPOINT();
    ptp2 = new TPOINT();
    if (ptp1 == NULL) {i_err = -1;goto error_tray;} // 内存不足
    if (ptp2 == NULL) {i_err = -1;goto error_tray;} // 内存不足
    ptp1->next = ptp1->prev = NULL;
    ptp1->ppoint = pep1;
    ptp2->next = ptp2->prev = NULL;
    ptp2->ppoint = pep2;


    pTSTHPSV->tps1.pPoints = ptp1;
    pTSTHPSV->tps2.pPoints = ptp2;

    // 获取端点集1相关的变化
    pTSTHPSV->tps1.deltas.deltaR.bValid = true;
    pTSTHPSV->tps1.deltas.deltaR.iMin = abs(pvmes[p2.n].R - pvmes[p1.n].R);
    pTSTHPSV->tps1.deltas.deltaR.iMax = abs(pvmes[p2.n].R - pvmes[p1.n].R);
    pTSTHPSV->tps1.deltas.deltaR.iSign = Sign(pvmes[p2.n].R - pvmes[p1.n].R);

    pTSTHPSV->tps1.deltas.deltaG.bValid = true;
    pTSTHPSV->tps1.deltas.deltaG.iMin = abs(pvmes[p2.n].G - pvmes[p1.n].G);
    pTSTHPSV->tps1.deltas.deltaG.iMax = abs(pvmes[p2.n].G - pvmes[p1.n].G);
    pTSTHPSV->tps1.deltas.deltaG.iSign = Sign(pvmes[p2.n].G - pvmes[p1.n].G);

    pTSTHPSV->tps1.deltas.deltaB.bValid = true;
    pTSTHPSV->tps1.deltas.deltaB.iMin = abs(pvmes[p2.n].B - pvmes[p1.n].B);
    pTSTHPSV->tps1.deltas.deltaB.iMax = abs(pvmes[p2.n].B - pvmes[p1.n].B);
    pTSTHPSV->tps1.deltas.deltaB.iSign = Sign(pvmes[p2.n].B - pvmes[p1.n].B);

    pTSTHPSV->tps1.deltas.deltaY.bValid = true;
    pTSTHPSV->tps1.deltas.deltaY.iMin = abs(pvmes[p2.n].Y - pvmes[p1.n].Y);
    pTSTHPSV->tps1.deltas.deltaY.iMax = abs(pvmes[p2.n].Y - pvmes[p1.n].Y);
    pTSTHPSV->tps1.deltas.deltaY.iSign = Sign(pvmes[p2.n].Y - pvmes[p1.n].Y);

    // 获取端点集2相关的变化
    pTSTHPSV->tps2.deltas.deltaR.bValid = true;
    pTSTHPSV->tps2.deltas.deltaR.iMin = abs(pvmes[p1.n].R - pvmes[p2.n].R);
    pTSTHPSV->tps2.deltas.deltaR.iMax = abs(pvmes[p1.n].R - pvmes[p2.n].R);
    pTSTHPSV->tps2.deltas.deltaR.iSign = Sign(pvmes[p1.n].R - pvmes[p2.n].R);

    pTSTHPSV->tps2.deltas.deltaG.bValid = true;
    pTSTHPSV->tps2.deltas.deltaG.iMin = abs(pvmes[p1.n].G - pvmes[p2.n].G);
    pTSTHPSV->tps2.deltas.deltaG.iMax = abs(pvmes[p1.n].G - pvmes[p2.n].G);
    pTSTHPSV->tps2.deltas.deltaG.iSign = Sign(pvmes[p1.n].G - pvmes[p2.n].G);

    pTSTHPSV->tps2.deltas.deltaB.bValid = true;
    pTSTHPSV->tps2.deltas.deltaB.iMin = abs(pvmes[p1.n].B - pvmes[p2.n].B);
    pTSTHPSV->tps2.deltas.deltaB.iMax = abs(pvmes[p1.n].B - pvmes[p2.n].B);
    pTSTHPSV->tps2.deltas.deltaB.iSign = Sign(pvmes[p1.n].B - pvmes[p2.n].B);

    pTSTHPSV->tps2.deltas.deltaY.bValid = true;
    pTSTHPSV->tps2.deltas.deltaY.iMin = abs(pvmes[p1.n].Y - pvmes[p2.n].Y);
    pTSTHPSV->tps2.deltas.deltaY.iMax = abs(pvmes[p1.n].Y - pvmes[p2.n].Y);
    pTSTHPSV->tps2.deltas.deltaY.iSign = Sign(pvmes[p1.n].Y - pvmes[p2.n].Y);

    pTSTHPSV->tps1.iTotal = 1;
    pTSTHPSV->tps2.iTotal = 1;

    pTSTHPSV->iTotal = 2;
    pTSTHPSV->iEPTotal = 2;
    pTSTHPSV->iMinIndex = (p1.n <= p2.n)?p1.n:p2.n;
    pTSTHPSV->iMaxIndex = (p1.n >= p2.n)?p1.n:p2.n;

    *ppTSTHPSV = pTSTHPSV;

    if(DeltasRelation(pTSTHPSV->tps2.deltas,pTSTHPSV->tps2.deltas) == 0){
        ptp12 = new TPOINT();
        ptp22 = new TPOINT();
        if (ptp12 == NULL) {i_err = -1;goto error_tray;} // 内存不足
        if (ptp22 == NULL) {i_err = -1;goto error_tray;} // 内存不足
        ptp12->next = ptp1->prev = NULL;
        ptp12->ppoint = pep2;
        ptp22->next = ptp2->prev = NULL;
        ptp22->ppoint = pep1;

        pTSTHPSV->tps1.pPoints->next = ptp12;ptp12->prev = pTSTHPSV->tps1.pPoints;

        pTSTHPSV->tps2.pPoints = ptp22;ptp2->prev = ptp2->next = NULL;
        pTSTHPSV->tps2.pPoints->next = ptp2;ptp2->prev = pTSTHPSV->tps2.pPoints;

        pTSTHPSV->tps1.iTotal = pTSTHPSV->tps2.iTotal = 2;
    }

    return 0;

error_tray:
    if (pTSTHPSV != NULL) delete pTSTHPSV;
    if (pep1 != NULL) delete pep1;
    if (pep2 != NULL) delete pep2;
    if (ptp1 != NULL) delete ptp1;
    if (ptp2 != NULL) delete ptp2;
    return i_err;
}

bool IsIn(CVisionMatterElementSet &vmes,TSTHPSV &tsth, POINT &point,PEPOINT *pret)
{    
    // 如果该点不在该同一物的点集范围内，则直接返回false
    if( (point.n > tsth.iMaxIndex) || (point.n < tsth.iMinIndex) ){
        return false;
    }

    // 否则，须遍历边缘点链表来找到该点
    PEPOINT cur = tsth.pEPS;
    for(cur = tsth.pEPS; cur != NULL; cur = cur->next){
        if(cur->point.n == point.n){
            *pret = cur;
            return true;
        }
    }
    return false;
}

int IterateVMES(CVisionMatterElementSet &vmes)
{
    PSVisionSymbolElement pdata = vmes.get_vmes_data();
    if (pdata == NULL) {
        return -1; // 数据无效
    }
    int w = 0;int h = 0;
    w = vmes.get_width();
    h = vmes.get_height();

    POINT p1, p2;

    int iret = 0;

    int x = 0;int y = 0;PTSTHPSV pTSTHPSV;
    for (y = 0; y < h; y++){
        for (x = 0; x < w; x++){
            p1.x = x;p1.y = y;p1.n = y * w + x;

            p2.x = x + 1;p2.y = y;p2.n = p2.y * w + p2.x;
            if (CreateTSTHPSV(vmes,p1,p2,&pTSTHPSV) == 0){
                iret = UploadTSTHPSV(vmes,pTSTHPSV);
            }

            p2.x = x - 1;p2.y = y + 1;p2.n = p2.y * w + p2.x;
            if (CreateTSTHPSV(vmes,p1,p2,&pTSTHPSV) == 0){
                iret = UploadTSTHPSV(vmes,pTSTHPSV);
            }

            p2.x = x;p2.y = y + 1;p2.n = p2.y * w + p2.x;
            if (CreateTSTHPSV(vmes,p1,p2,&pTSTHPSV) == 0){
                iret = UploadTSTHPSV(vmes,pTSTHPSV);
            }

            p2.x = x + 1;p2.y = y + 1;p2.n = p2.y * w + p2.x;
            if (CreateTSTHPSV(vmes,p1,p2,&pTSTHPSV) == 0){
                iret = UploadTSTHPSV(vmes,pTSTHPSV);
            }
        }
    }

    return 0;
}

// 计算两个变化（A，B）之间的关系,如果B==A，则返回0；如果B完全属于A，则返回1；否则返回-1
int DeltaRelation(DELTA &A, DELTA &B, int &i_signc){
    int i_drc = -1;

    if (!A.bValid) {
        if(!B.bValid){i_drc = 0;}else{i_drc = 1;}
    } else {
        if(!B.bValid){i_drc = -1;}else{
            if((B.iMin >= A.iMax) && (B.iMax <= A.iMax)){
                if((B.iMin == A.iMax) && (B.iMax == A.iMax)){
                    i_drc = 0;
                } else {
                    i_drc = 1;
                }
            } else {i_drc = -1;}
        }
    }

    i_signc = A.iSign * B.iSign;

    return i_drc;
}
// 计算两个变化（A，B）之间的关系,如果B==A，则返回0；如果B完全属于A，则返回1；否则返回-1
int DeltasRelation(DELTAS &A, DELTAS &B)
{
    int i_dr = -1;
    int i_sign = 0;
    int i_signc = 0;

    i_dr = DeltaRelation(A.deltaR, B.deltaR, i_signc);
    if(i_dr < 0) return -1;
    i_sign = i_signc;

    i_dr = DeltaRelation(A.deltaG, B.deltaG, i_signc);
    if(i_dr < 0) return -1;
    if (i_signc * i_sign < 0) return -1;
    i_sign = Sign(i_sign + i_signc);

    i_dr = DeltaRelation(A.deltaB, B.deltaB, i_signc);
    if(i_dr < 0) return -1;
    if (i_signc * i_sign < 0) return -1;
    i_sign = Sign(i_sign + i_signc);

    i_dr = DeltaRelation(A.deltaY, B.deltaY, i_signc);
    if(i_dr < 0) return -1;
    if (i_signc * i_sign < 0) return -1;
    i_sign = Sign(i_sign + i_signc);

    return i_dr;
}
int GetDELTASFromTSTHPSV(PTSTHPSV pTSTHPSV, int i,DELTAS *pDeltas)
{
     if( (pTSTHPSV->tps1.pPoints->ppoint->point.n == i) && (pTSTHPSV->tps1.pPoints->ppoint != NULL) ) {
         *pDeltas = pTSTHPSV->tps1.deltas;
         return 0;
     } else {
        if( (pTSTHPSV->tps2.pPoints->ppoint->point.n == i) && (pTSTHPSV->tps2.pPoints->ppoint != NULL) ) {
            *pDeltas = pTSTHPSV->tps2.deltas;
            return 0;
        }else{return -1;}
     }

     return -1;
}

int GetDELTSign(PSVisionSymbolElement pvmes, int n1, int n2, DELTAS &deltas,DELTAS *pdelta2)
{
    pdelta2->deltaR.bValid = deltas.deltaR.bValid;
    if(pdelta2->deltaR.bValid) pdelta2->deltaR.iSign =  Sign(pvmes[n1].R - pvmes[n2].R);

    pdelta2->deltaG.bValid = deltas.deltaG.bValid;
    if(pdelta2->deltaG.bValid) pdelta2->deltaG.iSign =  Sign(pvmes[n1].G - pvmes[n2].G);

    pdelta2->deltaB.bValid = deltas.deltaB.bValid;
    if(pdelta2->deltaB.bValid) pdelta2->deltaB.iSign =  Sign(pvmes[n1].B - pvmes[n2].B);

    pdelta2->deltaY.bValid = deltas.deltaY.bValid;
    if(pdelta2->deltaY.bValid) pdelta2->deltaY.iSign =  Sign(pvmes[n1].Y - pvmes[n2].Y);

    return 0;
}

bool IsUnBroken(DELTAS delta1, DELTAS delta2)
{
    if(delta2.deltaR.bValid) {
        if(delta2.deltaR.iSign * delta1.deltaR.iSign < 0) return false;
    }
    if(delta2.deltaG.bValid) {
        if(delta2.deltaG.iSign * delta1.deltaG.iSign < 0) return false;
    }
    if(delta2.deltaB.bValid) {
        if(delta2.deltaB.iSign * delta1.deltaB.iSign < 0) return false;
    }
    if(delta2.deltaY.bValid) {
        if(delta2.deltaY.iSign * delta1.deltaY.iSign < 0) return false;
    }
    return true;
}

// 判断某点是否是个一个合格的点（在w,h范围内）
bool IsReasonable(int w, int h, int x, int y)
{
    if( (x < 0) || (x >= w) ) return false;
    if( (y < 0) || (y >= h) ) return false;
    return true;
}

// 查看pCurCTSTHPSV上点pep的所有变化，统计这些变化中与pTSTHPSV中的变化相连续的数目
int GetUnbrokenNubmer(CVisionMatterElementSet &vmes, CTSTHPSV *pCurCTSTHPSV, PTSTHPSV pTSTHPSV,PEPOINT pep)
{
    int dncr = 0;

    DELTAS delta1,delta2;
    GetDELTASFromTSTHPSV(pTSTHPSV,pep->point.n, &delta1);

    PSVisionSymbolElement pvmes = vmes.get_vmes_data();

    int n1, n_not_n1, n2;
    n1 = pep->point.n;

    PEPOINT pep_tmp = NULL;
    for(PEPOINT pep_tmp = pTSTHPSV->pEPS;pep_tmp != NULL;pep_tmp = pep_tmp->next){
        if(pep_tmp->point.n != n1) {
            n_not_n1 = pep_tmp->point.n;break;
        }
    }

    bool b_unbroken;bool b_reasonable;

    if(pep->point.b_up > 0){
        if(pep->point.up != NULL) {n2 = pep->point.up->n;b_reasonable = true;}else{
            n2 = (pep->point.y - 1) * vmes.get_width() + (pep->point.x);
            b_reasonable = IsReasonable(vmes.get_width(),vmes.get_height(),
                         pep->point.x,pep->point.y - 1);
        }
        if ( (n2 != n_not_n1) && (b_reasonable == true) ){
            GetDELTSign(pvmes, n1, n2,  pCurCTSTHPSV->ptsth->tps1.deltas, &delta2);
            b_unbroken = IsUnBroken(delta1,delta2);
            if(b_unbroken) dncr++;
        }
    }

    if(pep->point.b_down > 0){
        if(pep->point.down != NULL) {n2 = pep->point.down->n;b_reasonable = true;}else{
            n2 = (pep->point.y + 1) * vmes.get_width() + (pep->point.x);
            b_reasonable = IsReasonable(vmes.get_width(),vmes.get_height(),
                         pep->point.x,pep->point.y + 1);
        }
        if ( (n2 != n_not_n1) && (b_reasonable == true) ){
            GetDELTSign(pvmes, n1, n2,  pCurCTSTHPSV->ptsth->tps1.deltas, &delta2);
            b_unbroken = IsUnBroken(delta1,delta2);
            if(b_unbroken) dncr++;
        }
    }

    if(pep->point.b_left > 0){
        if(pep->point.left != NULL) {n2 = pep->point.left->n;b_reasonable = true;}else{
            n2 = (pep->point.y) * vmes.get_width() + (pep->point.x - 1);
            b_reasonable = IsReasonable(vmes.get_width(),vmes.get_height(),
                         pep->point.x - 1,pep->point.y);
        }
        if ( (n2 != n_not_n1) && (b_reasonable == true) ){
            GetDELTSign(pvmes, n1, n2,  pCurCTSTHPSV->ptsth->tps1.deltas, &delta2);
            b_unbroken = IsUnBroken(delta1,delta2);
            if(b_unbroken) dncr++;
        }
    }

    if(pep->point.b_right > 0){
        if(pep->point.right != NULL) {n2 = pep->point.right->n;b_reasonable = true;}else{
            n2 = (pep->point.y) * vmes.get_width() + (pep->point.x + 1);
            b_reasonable = IsReasonable(vmes.get_width(),vmes.get_height(),
                         pep->point.x + 1, pep->point.y);
        }
        if ( (n2 != n_not_n1) && (b_reasonable == true) ){
            GetDELTSign(pvmes, n1, n2,  pCurCTSTHPSV->ptsth->tps1.deltas, &delta2);
            b_unbroken = IsUnBroken(delta1,delta2);
            if(b_unbroken) dncr++;
        }
    }

    if(pep->point.b_ru > 0){
        if(pep->point.ru != NULL) {n2 = pep->point.ru->n;b_reasonable = true;}else{
            n2 = (pep->point.y - 1) * vmes.get_width() + (pep->point.x + 1);
            b_reasonable = IsReasonable(vmes.get_width(),vmes.get_height(),
                         pep->point.x + 1,pep->point.y - 1);
        }
        if ( (n2 != n_not_n1) && (b_reasonable == true) ){
            GetDELTSign(pvmes, n1, n2,  pCurCTSTHPSV->ptsth->tps1.deltas, &delta2);
            b_unbroken = IsUnBroken(delta1,delta2);
            if(b_unbroken) dncr++;
        }
    }

    if(pep->point.b_rd > 0){
        if(pep->point.rd != NULL) {n2 = pep->point.rd->n;b_reasonable = true;}else{
            n2 = (pep->point.y + 1) * vmes.get_width() + (pep->point.x + 1);
            b_reasonable = IsReasonable(vmes.get_width(),vmes.get_height(),
                         pep->point.x + 1,pep->point.y + 1);
        }
        if ( (n2 != n_not_n1) && (b_reasonable == true) ){
            GetDELTSign(pvmes, n1, n2,  pCurCTSTHPSV->ptsth->tps1.deltas, &delta2);
            b_unbroken = IsUnBroken(delta1,delta2);
            if(b_unbroken) dncr++;
        }
    }

    if(pep->point.b_lu > 0){
        if(pep->point.lu != NULL) {n2 = pep->point.lu->n;b_reasonable = true;}else{
            n2 = (pep->point.y - 1) * vmes.get_width() + (pep->point.x - 1);
            b_reasonable = IsReasonable(vmes.get_width(),vmes.get_height(),
                         pep->point.x - 1,pep->point.y - 1);
        }
        if ( (n2 != n_not_n1) && (b_reasonable == true) ){
            GetDELTSign(pvmes, n1, n2,  pCurCTSTHPSV->ptsth->tps1.deltas, &delta2);
            b_unbroken = IsUnBroken(delta1,delta2);
            if(b_unbroken) dncr++;
        }
    }

    if(pep->point.b_ld > 0){
        if(pep->point.ld != NULL) {n2 = pep->point.ld->n;b_reasonable = true;}else{
            n2 = (pep->point.y + 1) * vmes.get_width() + (pep->point.x - 1);
            b_reasonable = IsReasonable(vmes.get_width(),vmes.get_height(),
                         pep->point.x - 1,pep->point.y + 1);
        }
        if ( (n2 != n_not_n1) && (b_reasonable == true) ){
            GetDELTSign(pvmes, n1, n2,  pCurCTSTHPSV->ptsth->tps1.deltas, &delta2);
            b_unbroken = IsUnBroken(delta1,delta2);
            if(b_unbroken) dncr++;
        }
    }

    return dncr;
}

int AddEdgePoint(CVisionMatterElementSet &vmes, CTSTHPSV *pCurCTSTHPSV, PTSTHPSV pTSTHPSV, PEPOINT pep)
{
    PEPOINT pCurEP = NULL;PEPOINT pLastEP = NULL;

    for(pCurEP = pCurCTSTHPSV->ptsth->pEPS;pCurEP != NULL; pCurEP = pCurEP->next){
        if( pep->point.n <= pCurEP->point.n) break;
        if( pCurEP->next == NULL) pLastEP = pCurEP;
    }

    if(pCurEP != NULL){
        pep->prev = pCurEP->prev;
        pep->next = pCurEP;
        if(pCurEP->prev == NULL) {
            pCurCTSTHPSV->ptsth->pEPS = pep;
            pCurCTSTHPSV->ptsth->iMinIndex = pep->point.n;
        }else{
            pCurEP->prev->next = pep;
            pCurEP->prev = pep;
        }
    }else{
        pLastEP->next = pep;
        pep->prev = pLastEP;
        pep->next = NULL;
        pCurCTSTHPSV->ptsth->iMaxIndex = pep->point.n;
    }

    pCurCTSTHPSV->ptsth->iEPTotal++;
    pCurCTSTHPSV->ptsth->iTotal++;

    return 0;
}

// 判定两个变化d1,d2是否同号如果返回0，则同号，否则异号
int IsIdentical(DELTAS &d1, DELTAS &d2)
{

    if(d1.deltaR.bValid){
        if( !(d2.deltaR.bValid) ){
            return -1;
        }else{
            if( d1.deltaR.iSign * d2.deltaR.iSign < 0 ) {
                return -1;
            }
        }
    }

    if(d1.deltaG.bValid){
        if( !(d2.deltaG.bValid) ){
            return -1;
        }else{
            if( d1.deltaG.iSign * d2.deltaG.iSign < 0 ) {
                return -1;
            }
        }
    }

    if(d1.deltaB.bValid){
        if( !(d2.deltaB.bValid) ){
            return -1;
        }else{
            if( d1.deltaB.iSign * d2.deltaB.iSign < 0 ) {
                return -1;
            }
        }
    }

    if(d1.deltaY.bValid){
        if( !(d2.deltaY.bValid) ){
            return -1;
        }else{
            if( d1.deltaY.iSign * d2.deltaY.iSign < 0 ) {
                return -1;
            }
        }
    }

    return 0;
}

int AddTermPoint(CVisionMatterElementSet &vmes, CTSTHPSV *pCurCTSTHPSV, PTSTHPSV pTSTHPSV, PEPOINT pep)
{
    DELTAS delta, d1, d2;
    GetDELTASFromTSTHPSV(pTSTHPSV, pep->point.n, &delta);
    d1 = pCurCTSTHPSV->ptsth->tps1.deltas;
    d2 = pCurCTSTHPSV->ptsth->tps2.deltas;

    PTPOINT p1 = NULL, p2 = NULL;

    if(IsIdentical(d1,delta) == 0) {// 如果d1,delta同号
        // 将pep加入pCurCTSTHPSV->ptsth->tps1
        p1 = new TPOINT();
        if(p1 == NULL) return -1; // 内存不足
        p1->next = p1->prev = NULL;
        p1->ppoint = pep;
        p1->next = pCurCTSTHPSV->ptsth->tps1.pPoints;

        if(pCurCTSTHPSV->ptsth->tps1.pPoints != NULL) {
            pCurCTSTHPSV->ptsth->tps1.pPoints->prev = p1;
        }
        pCurCTSTHPSV->ptsth->tps1.pPoints = p1;
        pCurCTSTHPSV->ptsth->tps1.iTotal++;
    }

    if(IsIdentical(d2,delta) == 0) {// 如果d2,delta同号
        // 将pep加入pCurCTSTHPSV->ptsth->tps2
        p2 = new TPOINT();
        if(p2 == NULL) {return -1;} // 内存不足
        p2->next = p2->prev = NULL;
        p2->ppoint = pep;
        p2->next = pCurCTSTHPSV->ptsth->tps2.pPoints;

        if(pCurCTSTHPSV->ptsth->tps2.pPoints != NULL) {
            pCurCTSTHPSV->ptsth->tps2.pPoints->prev = p2;
        }
        pCurCTSTHPSV->ptsth->tps2.pPoints = p2;
        pCurCTSTHPSV->ptsth->tps2.iTotal++;
    }

    return 0;
}

// 从视觉同一物中删除某个端点
int DeleteTermPoint(CTSTHPSV *pCurCTSTHPSV, PEPOINT pep)
{
    PTPOINT pCurTP = NULL;
    PTPOINT pTmpTP = NULL;

    pCurTP = pCurCTSTHPSV->ptsth->tps1.pPoints;
    while(pCurTP != NULL){
        if(pCurTP->ppoint == pep){
            if(pCurTP->prev != NULL) {
                pCurTP->prev->next = pCurTP->next;
            }else{
                pCurCTSTHPSV->ptsth->tps1.pPoints = pCurTP->next;
            }
            if(pCurTP->next != NULL) pCurTP->next->prev = pCurTP->prev;
            pTmpTP = pCurTP->next;delete pCurTP;pCurTP = pTmpTP;
            pCurCTSTHPSV->ptsth->tps1.iTotal--;
        }else{
            pCurTP = pCurTP->next;
        }
    }

    pCurTP = pCurCTSTHPSV->ptsth->tps2.pPoints;
    while(pCurTP != NULL){
        if(pCurTP->ppoint == pep){
            if(pCurTP->prev != NULL) {
                pCurTP->prev->next = pCurTP->next;
            }else{
                pCurCTSTHPSV->ptsth->tps2.pPoints = pCurTP->next;
            }
            if(pCurTP->next != NULL) pCurTP->next->prev = pCurTP->prev;
            pTmpTP = pCurTP->next;delete pCurTP;pCurTP = pTmpTP;
            pCurCTSTHPSV->ptsth->tps2.iTotal--;
        }else{
            pCurTP = pCurTP->next;
        }
    }

    return 0;
}

// 判断delta是否是一个变化全为0的变化
bool IsNotChage(DELTAS delta)
{
    if( delta.deltaR.bValid && (delta.deltaR.iSign != 0) ) return false;
    if( delta.deltaG.bValid && (delta.deltaR.iSign != 0) ) return false;
    if( delta.deltaB.bValid && (delta.deltaR.iSign != 0) ) return false;
    if( delta.deltaY.bValid && (delta.deltaR.iSign != 0) ) return false;

    return true;
}

// 从视觉同一物中删除边缘点pep
int DeleteEdgePoint(CTSTHPSV *pCurCTSTHPSV, PEPOINT pep)
{

    PEPOINT pCurEP = NULL;

    for(pCurEP = pCurCTSTHPSV->ptsth->pEPS; pCurEP != NULL; pCurEP = pCurEP->next){
        if(pCurEP->point.up == &(pep->point)) pCurEP->point.up =NULL;
        if(pCurEP->point.down == &(pep->point)) pCurEP->point.down =NULL;
        if(pCurEP->point.right == &(pep->point)) pCurEP->point.right =NULL;
        if(pCurEP->point.left == &(pep->point)) pCurEP->point.left =NULL;
        if(pCurEP->point.ru == &(pep->point)) pCurEP->point.ru =NULL;
        if(pCurEP->point.rd == &(pep->point)) pCurEP->point.rd =NULL;
        if(pCurEP->point.lu == &(pep->point)) pCurEP->point.lu =NULL;
        if(pCurEP->point.ld == &(pep->point)) pCurEP->point.ld =NULL;
    }

    if(pep->prev != NULL){
        pep->prev->next = pep->next;
    }else{
        pCurCTSTHPSV->ptsth->pEPS = pep->next;
    }
    if(pep->next != NULL){
        pep->next->prev = pep->prev;
    }

    delete pep;

    pCurCTSTHPSV->ptsth->iEPTotal--;

    return 0;
}

// 尝试将同一物pTSTHPSV吸收进入pCurCTSTHPSV
int TryToAbsorb(CVisionMatterElementSet &vmes, CTSTHPSV *pCurCTSTHPSV, PTSTHPSV pTSTHPSV)
{
    //return 0;

    POINT p1,p2;
    int dncr1, dncr2; // 新增的穿过该点的变化数
    PEPOINT pep1,pep2;

    int n; // 在pCurCTSTHPSV之上，且有其他变化与pTSTHPSV之上变化相连续的点的数目
    //int n_in; // 在pCurCTSTHPSV之上，且也在pTSTHPSV之上的点的数目

    /*
    if(DeltasRelation(pTSTHPSV->tps1.deltas,pTSTHPSV->tps2.deltas) == 0) {
        p1 = pTSTHPSV->tps1.pPoints->ppoint->point;
        p2 = pTSTHPSV->tps1.pPoints->ppoint->next->point;
    }else{
        p1 = pTSTHPSV->tps1.pPoints->ppoint->point;
        p2 = pTSTHPSV->tps2.pPoints->ppoint->point;
    }
    */
    p1 = pTSTHPSV->pEPS->point;
    p2 = pTSTHPSV->pEPS->next->point;

    dncr1 = dncr2 = 0;
    pep1 = pep2 = NULL;

    n = 0; //n_in = 0;

    bool b_is_in;

    b_is_in = IsIn(vmes, *(pCurCTSTHPSV->ptsth),p1, &pep1);
    if(!b_is_in){
        pep1 = NULL;
        dncr1 = 0;
    } else {
        // 获取dncr1
        dncr1 = GetUnbrokenNubmer(vmes, pCurCTSTHPSV, pTSTHPSV, pep1);
        if (dncr1 > 0) n++;
        //n_in++;
    }

    b_is_in = IsIn(vmes, *(pCurCTSTHPSV->ptsth),p2, &pep2);
    if(!b_is_in){
        pep2 = NULL;
        dncr2 = 0;
    } else {
        // 获取dncr2
        dncr2 = GetUnbrokenNubmer(vmes, pCurCTSTHPSV, pTSTHPSV, pep2);
        if (dncr2 > 0) n++;
        //n_in++;
    }

    if (n > 0) { // 如果pTSTHPSV是pCurCTSTHPSV的一部分
        if(pep1 == NULL){
            // 创建一个pep1,并初始化其n_crossover，iCurNeighbour为0，并将其加入pCurCTSTHPSV
            pep1 = new EPOINT();if(pep1 == NULL) return -2; // 内存不足
            pep1->next = pep1->prev = NULL;
            pep1->n_crossover = 0;
            pep1->point.iCurNeighbour = 0;
            pep1->point = p1;
            pep1->point.iCurNeighbour = 0;

            if(pep1->point.b_up == 1) pep1->point.up = &(pep2->point);
            if(pep1->point.b_down == 1) pep1->point.down = &(pep2->point);
            if(pep1->point.b_left == 1) pep1->point.left = &(pep2->point);
            if(pep1->point.b_right == 1) pep1->point.right = &(pep2->point);
            if(pep1->point.b_ru == 1) pep1->point.ru = &(pep2->point);
            if(pep1->point.b_rd == 1) pep1->point.rd = &(pep2->point);
            if(pep1->point.b_lu == 1) pep1->point.lu = &(pep2->point);
            if(pep1->point.b_ld == 1) pep1->point.ld = &(pep2->point);

            if(p2.b_up == 1) {pep2->point.b_up = 1;pep2->point.up = &(pep1->point);}
            if(p2.b_down == 1) {pep2->point.b_down = 1;pep2->point.down = &(pep1->point);}
            if(p2.b_left == 1) {pep2->point.b_left = 1;pep2->point.left = &(pep1->point);}
            if(p2.b_right == 1) {pep2->point.b_right = 1;pep2->point.right = &(pep1->point);}
            if(p2.b_ru == 1) {pep2->point.b_ru = 1;pep2->point.ru = &(pep1->point);}
            if(p2.b_rd == 1) {pep2->point.b_rd = 1;pep2->point.rd = &(pep1->point);}
            if(p2.b_lu == 1) {pep2->point.b_lu = 1;pep2->point.lu = &(pep1->point);}
            if(p2.b_ld == 1) {pep2->point.b_ld = 1;pep2->point.ld = &(pep1->point);}

            // 将pep1新增到边缘点列表以及端点列表之中去
            AddEdgePoint(vmes, pCurCTSTHPSV, pTSTHPSV, pep1);
            if(AddTermPoint(vmes, pCurCTSTHPSV, pTSTHPSV, pep1) != 0) return -2; // 内存不足

        }
        if(pep2 == NULL) {
            // 创建一个pep2,并初始化其n_crossover，iCurNeighbour为0，并将其加入pCurCTSTHPSV
            pep2 = new EPOINT();if(pep2 == NULL) return -2; // 内存不足
            pep2->next = pep2->prev = NULL;
            pep2->n_crossover = 0;
            pep2->point.iCurNeighbour = 0;
            pep2->point = p2;
            pep2->point.iCurNeighbour = 0;

            if(pep2->point.b_up == 1) pep2->point.up = &(pep1->point);
            if(pep2->point.b_down == 1) pep2->point.down = &(pep1->point);
            if(pep2->point.b_left == 1) pep2->point.left = &(pep1->point);
            if(pep2->point.b_right == 1) pep2->point.right = &(pep1->point);
            if(pep2->point.b_ru == 1) pep2->point.ru = &(pep1->point);
            if(pep2->point.b_rd == 1) pep2->point.rd = &(pep1->point);
            if(pep2->point.b_lu == 1) pep2->point.lu = &(pep1->point);
            if(pep2->point.b_ld == 1) pep2->point.ld = &(pep1->point);

            if(p1.b_up == 1) {pep1->point.b_up = 1;pep1->point.up = &(pep2->point);}
            if(p1.b_down == 1) {pep1->point.b_down = 1;pep1->point.down = &(pep2->point);}
            if(p1.b_left == 1) {pep1->point.b_left = 1;pep1->point.left = &(pep2->point);}
            if(p1.b_right == 1) {pep1->point.b_right = 1;pep1->point.right = &(pep2->point);}
            if(p1.b_ru == 1) {pep1->point.b_ru = 1;pep1->point.ru = &(pep2->point);}
            if(p1.b_rd == 1) {pep1->point.b_rd = 1;pep1->point.rd = &(pep2->point);}
            if(p1.b_lu == 1) {pep1->point.b_lu = 1;pep1->point.lu = &(pep2->point);}
            if(p1.b_ld == 1) {pep1->point.b_ld = 1;pep1->point.ld = &(pep2->point);}

            // 将pep2新增到边缘点列表以及端点列表之中去
            AddEdgePoint(vmes, pCurCTSTHPSV, pTSTHPSV, pep2);
            if(AddTermPoint(vmes, pCurCTSTHPSV, pTSTHPSV, pep2) != 0) return -2; // 内存不足

        }

        pep1->n_crossover += dncr1;pep1->point.iCurNeighbour += 1;
        pep2->n_crossover += dncr2;pep2->point.iCurNeighbour += 1;

        if( ((pep1->n_crossover - dncr1) == 0) && (dncr1 > 0) ){
            // 将pep1从端点列表中去掉
            DeleteTermPoint(pCurCTSTHPSV, pep1);
        }
        if( ((pep2->n_crossover - dncr2) == 0) && (dncr2 > 0) ){
            // 将pep2从端点列表中去掉
            DeleteTermPoint(pCurCTSTHPSV, pep2);
        }

        // 根据pep1,pep2的iCurNeighbour及其变化类型来修正其是否从边缘点列表中去掉
        bool b_is_not_change = IsNotChage(pCurCTSTHPSV->ptsth->tps1.deltas);
        if( (pep1->point.iCurNeighbour >= pep1->point.iMaxNeighbour) &&
                b_is_not_change) {
            DeleteEdgePoint(pCurCTSTHPSV,pep1);
        }
        if( (pep2->point.iCurNeighbour >= pep2->point.iMaxNeighbour) &&
                b_is_not_change) {
            DeleteEdgePoint(pCurCTSTHPSV,pep2);
        }


        // pCurCTSTHPSV->ptsth->iTotal += (2 - n_in); // 应该是没有用了，这句

        return 0;
    } else {     // 如果pTSTHPSV不是pCurCTSTHPSV的一部分
        return -1;
    }

}

int UploadTSTHPSV(CVisionMatterElementSet &vmes,PTSTHPSV pTSTHPSV)
{
    int i_dr;
    int i_err;

    CTSTHPSV *pCTSTHPSV;

    i_err = 0;pCTSTHPSV = NULL;

    // 如果同一物点集列表为空
    if (g_tsthpsv_list == NULL) {
        pCTSTHPSV = new CTSTHPSV();
        if (pCTSTHPSV == NULL) {i_err = -1; goto err_tray;}; // 内存不足

        pCTSTHPSV->next = pCTSTHPSV->prev = NULL;
        pCTSTHPSV->pabs = NULL;pCTSTHPSV->ptsth = pTSTHPSV;

        g_tsthpsv_list = pCTSTHPSV;
        return 0;
    } //else {return 0;}

    // 遍历同一物点集列表
    bool b_addedto_list; // pTSTHPSV是否作为一个新的元素被加入g_tsthpsv_list
    b_addedto_list = true;
    CTSTHPSV *pCurCTSTHPSV;pCurCTSTHPSV = NULL;
    for(pCurCTSTHPSV = g_tsthpsv_list;pCurCTSTHPSV !=NULL;pCurCTSTHPSV = pCurCTSTHPSV->next){
        i_dr = DeltasRelation(pCurCTSTHPSV->ptsth->tps1.deltas, pTSTHPSV->tps1.deltas);
        if(i_dr < 0){
            ;// 目前为止，什么都不干，以后代码继续完善
        }else{
            if(TryToAbsorb(vmes, pCurCTSTHPSV, pTSTHPSV) == 0){ // 尝试合并吸收，如果成功
                if(i_dr == 0){
                    b_addedto_list = false;
                }
            }
        }

    }
    if(b_addedto_list == true){
        // 添加pTSTHPSV进g_tsthpsv_list
        pCTSTHPSV = new CTSTHPSV();
        if (pCTSTHPSV == NULL) {i_err = -1; goto err_tray;}; // 内存不足

        pCTSTHPSV->next = g_tsthpsv_list;
        pCTSTHPSV->prev = NULL;
        pCTSTHPSV->pabs = NULL;pCTSTHPSV->ptsth = pTSTHPSV;
        g_tsthpsv_list = pCTSTHPSV;
    } else {delete pTSTHPSV;}
    return 0;

    err_tray:
        if (pCTSTHPSV != NULL) delete pCTSTHPSV;
        return i_err;
}
